﻿using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Text;
using System.Threading;
using System.Diagnostics;

namespace SerialPortHelperLib
{
    public class Sensor_I2C_Ex
    {
        private const int COMMAND_TIMEOUT = 3000; // 3秒超时
        private readonly SerialPort sPortTemp;
        private readonly uint deviceAddress;
        private readonly I2CMode i2cMode;
        private readonly bool debugEnabled;
        private readonly object portLock = new object();
        private readonly Stopwatch stopwatch = new Stopwatch();

        public delegate void SendEventSensorHandler(string msg);
        public event SendEventSensorHandler SendSensorEvent;

        public enum I2CMode
        {
            Addr8Data8,
            Addr16Data8,
            Addr16Data16,
            Addr32Data8,
            Addr32Data16,
            Addr32Data32,

        }

        public Sensor_I2C_Ex(uint address, I2CMode mode, SerialPort serialPort, bool debug = true)
        {
            deviceAddress = address;
            i2cMode = mode;
            sPortTemp = serialPort;
            debugEnabled = debug;
        }

        #region 核心通信方法
        public bool SendRegDataARM(byte command, uint address, uint data)
        {
            byte[] sendBuffer = new byte[9];
            // 构建命令包
            sendBuffer[0] = command;
            Buffer.BlockCopy(BitConverter.GetBytes(address), 0, sendBuffer, 1, 4);
            Buffer.BlockCopy(BitConverter.GetBytes(data), 0, sendBuffer, 5, 4);

            lock (portLock)
            {
                try
                {
                    byte[] response = TransactSerial(sendBuffer, 1);
                    if (response == null || response.Length == 0 || response[0] != command)
                    {
                        DebugLog($"Error: Invalid response for command {command:X2}");
                        return false;
                    }
                    int byte_sel = (int)(address) & 0x3;
                    byte_sel = byte_sel * 8;
                    var reg_data = data >> byte_sel;
                    reg_data = reg_data & 0xff;
                    // 统一格式的日志输出
                    DebugLog($"W:0x{address:X8} 0x{reg_data:X2}");
                    return true;
                }
                catch (TimeoutException)
                {
                    DebugLog("Error: Command timed out");
                    return false;
                }
            }
        }

        public uint ReadRegDataARM(byte command, uint address)
        {
            byte[] sendBuffer = new byte[5];
            sendBuffer[0] = command;
            Buffer.BlockCopy(BitConverter.GetBytes(address), 0, sendBuffer, 1, 4);

            lock (portLock)
            {
                try
                {
                    byte[] response = TransactSerial(sendBuffer, 5);

                    // 检查响应是否有效
                    if (response == null || response.Length < 5)
                    {
                        DebugLog($"Error: Invalid response length for command {command:X2}");
                        return 0xFFFFFFFF;
                    }

                    // 获取地址的最后两位（字节选择位）
                    int byte_sel = (int)(address & 0x03);
                    uint result = 0;

                    // 处理特殊命令0x11（完整32位数据）
                    if (response[0] == 0x11)
                    {
                        result = BitConverter.ToUInt32(response, 1);
                    }
                    else
                    {
                        // 根据字节选择位置取对应的字节
                        switch (byte_sel)
                        {
                            case 0:
                                result = response[1];  // 第一个数据字节
                                break;
                            case 1:
                                result = response[2];  // 第二个数据字节
                                break;
                            case 2:
                                result = response[3];  // 第三个数据字节
                                break;
                            case 3:
                                result = response[4];  // 第四个数据字节
                                break;
                            default:
                                result = 0xFFFFFFFF;   // 无效选择
                                break;
                        }
                    }

                    // 统一格式的日志输出
                    DebugLog($"R:0x{address:X8} 0x{result:X2}");
                    return result;
                }
                catch (TimeoutException)
                {
                    DebugLog("Error: Command timed out");
                    return 0xFFFFFFFF;
                }
            }
        }

        private byte[] TransactSerial(byte[] sendData, int expectedResponseLength)
        {
            if (sPortTemp == null || !sPortTemp.IsOpen)
                throw new InvalidOperationException("Serial port is not open");

            sPortTemp.DiscardInBuffer();
            sPortTemp.Write(sendData, 0, sendData.Length);

            byte[] responseBuffer = new byte[expectedResponseLength];
            int bytesRead = 0;
            stopwatch.Restart();

            while (bytesRead < expectedResponseLength)
            {
                if (stopwatch.ElapsedMilliseconds > COMMAND_TIMEOUT)
                    throw new TimeoutException();

                int available = sPortTemp.BytesToRead;
                if (available > 0)
                {
                    int toRead = Math.Min(available, expectedResponseLength - bytesRead);
                    bytesRead += sPortTemp.Read(responseBuffer, bytesRead, toRead);
                }
                else
                {
                    // 短暂等待以避免CPU高占用
                    Thread.Sleep(1);
                }
            }

            return responseBuffer;
        }
        #endregion

        #region I2C 硬件操作
        public void EnableAE() => WriteRegister(0x20000032, 0x1);
        public void DisableAE() => WriteRegister(0x20000032, 0x0);

        public void SwitchToMaster()
        {
            uint current = ReadRegister(0x8050004e);
            WriteRegister(0x8050004e, current | 0x10);
        }

        public void SwitchToSlave()
        {
            uint current = ReadRegister(0x8050004e);
            WriteRegister(0x8050004e, current & ~0x10U);
        }

        public uint ReadRegister(uint address)
        {
            const uint baseAddr = 0x82000000;
            SendRegDataARM(0x1, baseAddr, 0);
            return ReadRegDataARM(0x31, address);
        }

        public void WriteRegister(uint address, uint value)
        {
            const uint baseAddr = 0x82000000;
            SendRegDataARM(0x1, baseAddr, 0);

            int byteShift = ((int)address & 0x3) * 8;
            uint maskedValue = (value & 0xFF) << byteShift;
            SendRegDataARM(0x21, address, maskedValue);
        }

        public void WriteSensorRegister(uint regAddr, uint regValue)
        {
            try
            {
                //DisableAE();
                //Thread.Sleep(30);
                SwitchToMaster();
                ConfigureI2CPins();

                switch (i2cMode)
                {
                    case I2CMode.Addr8Data8:
                        I2CWrite8(regAddr, regValue);
                        break;
                    case I2CMode.Addr16Data8:
                        I2CWrite16_8(regAddr, regValue);
                        break;
                    case I2CMode.Addr16Data16:
                        I2CWrite16_16(regAddr, regValue);
                        break;
                }

                // 统一格式的日志输出
                DebugLog($"W:0x{regAddr:X4} 0x{regValue:X2}");
            }
            finally
            {
                SwitchToSlave();
                EnableAE();
            }
        }

        public uint ReadSensorRegister(uint regAddr)
        {
            try
            {
                //DisableAE();
                //Thread.Sleep(30);
                SwitchToMaster();
                ConfigureI2CPins();

                uint result;
                switch (i2cMode)
                {
                    case I2CMode.Addr8Data8:
                        result = I2CRead8(regAddr);
                        break;
                    case I2CMode.Addr16Data8:
                        result = I2CRead16_8(regAddr);
                        break;
                    case I2CMode.Addr16Data16:
                        result = I2CRead16_16(regAddr);
                        break;
                    default:
                        result = 0xFFFFFFFF;
                        break;
                }

                // 统一格式的日志输出
                DebugLog($"R:0x{regAddr:X4} 0x{result:X2}");
                return result;
            }
            finally
            {
                SwitchToSlave();
                EnableAE();
            }
        }

        public void BatchWriteRegisters(IEnumerable<KeyValuePair<uint, uint>> registers)
        {
            try
            {
                DisableAE();
                Thread.Sleep(30);
                SwitchToMaster();
                ConfigureI2CPins();

                foreach (var reg in registers)
                {
                    switch (i2cMode)
                    {
                        case I2CMode.Addr8Data8:
                            I2CWrite8(reg.Key, reg.Value);
                            break;
                        case I2CMode.Addr16Data8:
                            I2CWrite16_8(reg.Key, reg.Value);
                            break;
                        case I2CMode.Addr16Data16:
                            I2CWrite16_16(reg.Key, reg.Value);
                            break;
                    }
                    // 统一格式的日志输出
                    DebugLog($"W:0x{reg.Key:X4} 0x{reg.Value:X2}");
                }
            }
            finally
            {
                SwitchToSlave();
                EnableAE();
            }
        }
        #endregion

        #region I2C 底层操作
        private void ConfigureI2CPins()
        {
            uint regAddr = 0x8050004d;
            uint current = ReadRegister(regAddr);
            WriteRegister(regAddr, current & ~0x00000002U);
        }

        private void I2CWrite8(uint addr, uint data)
        {
            I2CReady();
            WriteI2CReg(0x05, addr);
            WriteI2CReg(0x06, data);
            WriteI2CReg(0x04, 0x37);
        }

        private void I2CWrite16_8(uint addr, uint data)
        {
            I2CReady();
            WriteI2CReg(0x05, (addr >> 8) & 0xFF);
            WriteI2CReg(0x06, addr & 0xFF);
            WriteI2CReg(0x04, 0x17);
            //I2CReady();
            WriteI2CReg(0x06, data);
            WriteI2CReg(0x04, 0x24);
        }

        private void I2CWrite16_16(uint addr, uint data)
        {
            WriteI2CReg(0x05, (addr >> 8) & 0xFF);
            WriteI2CReg(0x06, addr & 0xFF);
            WriteI2CReg(0x04, 0x17);
            I2CReady();
            WriteI2CReg(0x05, (data >> 8) & 0xFF);
            WriteI2CReg(0x06, data & 0xFF);
            WriteI2CReg(0x04, 0x26);
        }

        private uint I2CRead8(uint addr)
        {
            I2CReady();
            WriteI2CReg(0x05, addr);
            WriteI2CReg(0x04, 0x33);
            Thread.Sleep(2);
            WriteI2CReg(0x04, 0xF9);
            Thread.Sleep(2);
            return ReadI2CReg(0x07);
        }

        private uint I2CRead16_8(uint addr)
        {
            I2CReady();
            WriteI2CReg(0x05, (addr >> 8) & 0xFF);
            WriteI2CReg(0x06, addr & 0xFF);
            WriteI2CReg(0x04, 0x37);
            //I2CReady();
            WriteI2CReg(0x04, 0xF9);
            return ReadI2CReg(0x07);
        }

        private uint I2CRead16_16(uint addr)
        {
            WriteI2CReg(0x05, (addr >> 8) & 0xFF);
            WriteI2CReg(0x06, addr & 0xFF);
            WriteI2CReg(0x04, 0x17);
            Thread.Sleep(10);
            WriteI2CReg(0x04, 0x59);
            Thread.Sleep(10);
            uint val1 = ReadI2CReg(0x07);
            WriteI2CReg(0x04, 0xA8);
            uint val2 = ReadI2CReg(0x07);
            return (val1 << 8) | val2;
        }

        private void I2CReady()
        {
            stopwatch.Restart();
            while ((ReadI2CReg(0x08) & 0x01) != 0)
            {
                if (stopwatch.ElapsedMilliseconds > 3000)
                {
                    DebugLog("Error: I2C timeout");
                    break;
                }
                Thread.Sleep(1);
            }
        }

        private void WriteI2CReg(uint reg, uint value) =>
            WriteRegister(0x80250000 + reg, value);

        private uint ReadI2CReg(uint reg) =>
            ReadRegister(0x80250000 + reg);
        #endregion

        #region 辅助方法
        private void DebugLog(string message)
        {
            if (debugEnabled && SendSensorEvent != null)
            {
                SendSensorEvent(message);
            }
        }
        #endregion
    }
}

// 高精度计时器封装
public class StopwatchEx
    {
        private System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

        public void Restart()
        {
            sw.Restart();
        }

        public long ElapsedMilliseconds => sw.ElapsedMilliseconds;
    }


